#include <iostream>
#include <motor_CAN.hpp>

using namespace std;

int main()
{
    cout << "Start Motor Can Test" << endl;

    auto server = make_shared<CanServer>();
    if (!server->StartServer("can0"))
    {
        cout << "Open Can Server fail." << endl;
        return -1;
    }

    Motor_Can motor;
    MotorConfig cfg{.address = 0x70, .poleCount = 15};
    if (!motor.InitDevice(server, cfg))
    {
        cout << "Init motor fail" << endl;
        return -1;
    }

    while (true)
    {
        auto start = chrono::steady_clock::now();
        motor.RequestRPM();
        motor.SendHeartBeat();
        // motor.SetCurrent(1000);
        cout << "rpm:" << motor.GetRPM() << endl;
        auto now = chrono::steady_clock::now();
        int us = chrono::duration_cast<chrono::microseconds>(now - start).count();
        cout << "fps:" << 1000000 / us << endl;
        this_thread::sleep_for(chrono::milliseconds(50));
    }
    return 0;
}